/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Andre Meixner
* @copyright  2018 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#ifndef __MMM_ATTACHEDSENSOR_H_
#define __MMM_ATTACHEDSENSOR_H_

#include "InterpolatableSensor.h"

namespace MMM {

class IAttachedSensor;

typedef boost::shared_ptr<IAttachedSensor> IAttachedSensorPtr;

class MMM_IMPORT_EXPORT IAttachedSensor {
public:
    static void loadSensorConfiguration(IAttachedSensorPtr attachedSensor, RapidXMLReaderNodePtr configurationNode) {
        attachedSensor->loadConfigurationXML(configurationNode);
    }

    virtual void setSegment(const std::string &segment) = 0;
    virtual std::string getSegment() = 0;

    virtual void setOffsetUnit(const std::string &offsetUnit) = 0;
    virtual std::string getOffsetUnit() = 0;

    virtual void setOffset(const Eigen::Matrix4f &offset) = 0;
    virtual Eigen::Matrix4f getOffset() = 0;

    virtual void safeSensorConfiguration(const std::string &filePath) = 0;

protected:
    virtual void loadConfigurationXML(RapidXMLReaderNodePtr node) = 0;
};

template <typename M>
class MMM_IMPORT_EXPORT AttachedSensor : public virtual InterpolatableSensor<M>, public IAttachedSensor {
public:
    void safeSensorConfiguration(const std::string &filePath) {
        RapidXMLWriterPtr writer(new RapidXMLWriter());
        RapidXMLWriterNodePtr configurationNode = writer->createRootNode("Configuration");
        appendConfigurationElementsXML(configurationNode);
        writer->saveToFile(filePath, true);
    }

    bool checkModel(ModelPtr model) {
        if (!model) {
            MMM_ERROR << "Sensor '" << this->uniqueName << "' needs a model."  << std::endl;
            return false;
        }
        if (model->getModelNode(segment)) return true;
        else return false;
    }

    bool equalsConfiguration(SensorPtr other) {
        boost::shared_ptr<AttachedSensor<M> > ptr = boost::dynamic_pointer_cast<AttachedSensor>(other);
        if (ptr) {
            return segment == ptr->segment && offset == ptr->offset && offsetUnit == ptr->offsetUnit;
        }
        else return false;
    }

    bool hasSensorConfigurationContent() const {
        return true;
    }

    void setSegment(const std::string &segment) {
        this->segment = segment;
    }

    std::string getSegment() {
        return segment;
    }

    void setOffsetUnit(const std::string &offsetUnit) {
        this->offsetUnit = offsetUnit;
    }

    std::string getOffsetUnit() {
        return offsetUnit;
    }

    void setOffset(const Eigen::Matrix4f &offset) {
        this->offset = offset;
    }

    Eigen::Matrix4f getOffset() {
        return offset;
    }

protected:
    AttachedSensor() : InterpolatableSensor<M>()
    {
    }

    AttachedSensor(const std::string &segment, const std::string &offsetUnit = "mm", const Eigen::Matrix4f &offset = Eigen::Matrix4f::Identity(), const std::string &description = std::string()) :
        InterpolatableSensor<M>(description),
        segment(segment),
        offsetUnit(offsetUnit),
        offset(offset)
    {
    }

    void loadConfigurationXML(RapidXMLReaderNodePtr node) {
        assert(node->name() == "Configuration");

        segment = node->first_node("Segment")->attribute_value("name");
        if (node->has_node("Offset")) {
            RapidXMLReaderNodePtr offsetNode = node->first_node("Offset");
            offsetUnit = offsetNode->attribute_value("unit");
            offset = XML::process4x4Matrix(offsetNode->first_node("Matrix4x4"));
        } else {
            offsetUnit = "mm";
            offset = Eigen::Matrix4f::Identity();
        }
    }

    void appendConfigurationXML(RapidXMLWriterNodePtr node) {
        RapidXMLWriterNodePtr configurationNode = node->append_node("Configuration");
        appendConfigurationElementsXML(configurationNode);
    }

    void appendConfigurationElementsXML(RapidXMLWriterNodePtr configurationNode) {
        configurationNode->append_node("Segment")->append_attribute("name", segment);
        RapidXMLWriterNodePtr offsetNode = configurationNode->append_node("Offset");
        offsetNode->append_attribute("unit", offsetUnit);
        XML::appendXML(offset, offsetNode, "Matrix4x4");
    }

    std::string segment;
    std::string offsetUnit;
    Eigen::Matrix4f offset;
};

}

#endif // __MMM_ATTACHEDSENSOR_H_
